#include <math.h>
#include <iostream>
#include "dynamicvoronoi.h"

DynamicVoronoi::DynamicVoronoi() {
  sqrt2 = sqrt(2.0);
  mData = NULL;
  gridMap = NULL;
  alternativeDiagram = NULL;
  allocatedGridMap = false;
}

DynamicVoronoi::~DynamicVoronoi() {
  if (mData) {
    for (int x=0; x<sizeX; x++) delete[] mData[x];
    delete[] mData;
  }
  if (allocatedGridMap && gridMap) {
    for (int x=0; x<sizeX; x++) delete[] gridMap[x];
    delete[] gridMap;
  }
}

void DynamicVoronoi::initializeEmpty(int _sizeX, int _sizeY, bool initGridMap) 
{
  if (mData) {
    for (int x = 0; x < sizeX; x++)
      delete[] mData[x];
    delete[] mData;
    mData = NULL;
  }

  if(alternativeDiagram){
    for (int x = 0; x < sizeX; x++)
      delete[] alternativeDiagram[x];
    delete[] alternativeDiagram;
    alternativeDiagram = NULL;
  }

  if (initGridMap) 
  {
    if (allocatedGridMap && gridMap) {
      for (int x = 0; x < sizeX; x++)
        delete[] gridMap[x];
      delete[] gridMap;
      gridMap = NULL;
      allocatedGridMap = false;
    }
  }

  sizeX = _sizeX;
  sizeY = _sizeY;
  mData = new dataCell*[sizeX];
  for (int x = 0; x < sizeX; x++)
    mData[x] = new dataCell[sizeY];

  if (initGridMap) {
    gridMap = new bool*[sizeX];
    for (int x = 0; x < sizeX; x++)
      gridMap[x] = new bool[sizeY];
    allocatedGridMap = true;
  }
  
  dataCell c;
  c.dist = INFINITY;          // 初始化为无穷大INFINITY，表示该点到障碍物点的初始距离未知
  c.sqdist = INT_MAX;         // 初始化为无穷大INFINITY，该点到起点的初始平方距离未知
  c.obstX = invalidObstData;  // 初始状态的障碍物坐标点，设置为最大值，一般自由区域不会改变该值
  c.obstY = invalidObstData;
  c.voronoi = free;           // 0  初始状态为 free，表示该栅格为自由区域
  c.queueing = fwNotQueued;   // 1  初始状态为 fwNotQueued, 表示该栅格未加入到任何集合中
  c.needsRaise = false;       // 0  表示该栅格不需要去除障碍物属性

  for (int x=0; x<sizeX; x++)
    for (int y = 0; y < sizeY; y++)
      mData[x][y] = c;

  if (initGridMap) {
    for (int x=0; x<sizeX; x++)
      for (int y = 0; y < sizeY; y++)
        gridMap[x][y] = 0;
  }

  return;
}

// 
void DynamicVoronoi::initializeMap(int _sizeX, int _sizeY, bool** _gridMap) 
{
  gridMap = _gridMap;
  initializeEmpty(_sizeX, _sizeY, false);
  for (int x = 0; x < sizeX; x++)
  {
    for (int y = 0; y < sizeY; y++)
    {
      // 障碍物区域为true，这里只对障碍物区域进行处理
      if (gridMap[x][y]) 
      {
        dataCell c = mData[x][y];
        if (!isOccupied(x, y, c))
        {
          bool isSurrounded = true;         // 判断周围的点是否为自由区域
          for (int dx = -1; dx <= 1; dx++)
          {
            int nx = x + dx;
            if (nx <= 0 || nx >= sizeX - 1)
              continue;
            for (int dy = -1; dy <= 1; dy++){
              if (dx == 0 && dy == 0)
                continue;
              int ny = y + dy;
              if (ny <= 0 || ny >= sizeY - 1)
                continue;

              if (!gridMap[nx][ny]) {
                isSurrounded = false;
                break;
              }
            }
          }

          if (isSurrounded) {
            c.obstX = x;
            c.obstY = y;
            c.sqdist = 0;
            c.dist = 0;
            c.voronoi = occupied;
            c.queueing = fwProcessed; // 该点本身是障碍物并且它周围一圈也都是障碍物，将其标记为已处理的点
            mData[x][y] = c;
          }
          else
            setObstacle(x, y);        // 该点本身是障碍物并且它周围一圈存在自由区域，将加入到addList中
        }
      }
    }
  }
  return;
}

void DynamicVoronoi::occupyCell(int x, int y) {
  gridMap[x][y] = 1;
  setObstacle(x,y);
}

void DynamicVoronoi::clearCell(int x, int y) {
  gridMap[x][y] = 0;
  removeObstacle(x,y);
}

// 将坐标点加入到addList集合 并且 mData内的值也进行更新
void DynamicVoronoi::setObstacle(int x, int y) 
{
  dataCell curr = mData[x][y];

  // 判断当前坐标点 是否等于 该点对应的栅格的障碍物坐标点
  if (isOccupied(x, y, curr))
    return;

  addList.push_back(INTPOINT(x,y));
  curr.obstX = x;
  curr.obstY = y;
  mData[x][y] = curr;

  return;
}

void DynamicVoronoi::removeObstacle(int x, int y) 
{
  dataCell c = mData[x][y];
  if (isOccupied(x, y, c) == false)
    return;

  removeList.push_back(INTPOINT(x,y));  // 将障碍物点加入到集合中
  c.obstX = invalidObstData;
  c.obstY  = invalidObstData;    
  c.queueing = bwQueued;
  mData[x][y] = c;
}

// 增加障碍物点
void DynamicVoronoi::exchangeObstacles(std::vector<INTPOINT>& points) {

  int idd = 0;
  for (unsigned int i = 0; i < lastObstacles.size(); i++){
    int x = lastObstacles[i].x;
    int y = lastObstacles[i].y;

    bool v = gridMap[x][y]; // 记录障碍物的个数
    if (v)
      continue;
    
    idd++;
    removeObstacle(x, y);
  }
  std::cout << "lastObstacles_size: " << lastObstacles.size() << " ,idd: " << idd << std::endl;
  lastObstacles.clear();

  for (unsigned int i = 0; i < points.size(); i++){
    int x = points[i].x;
    int y = points[i].y;
    bool v = gridMap[x][y];
    if (v)
      continue;
    
    setObstacle(x, y);
    lastObstacles.push_back(points[i]);   // 记录这一次的障碍物点，供给下一次使用
  }
  return;
}

void DynamicVoronoi::update(bool updateRealDist) 
{
  commitAndColorize(updateRealDist);

  while (!mOpen.empty())
  {
    INTPOINT p = mOpen.pop();    // 取出集合中完全平方距离障碍物最短的点
    int x = p.x;
    int y = p.y;
    dataCell cell = mData[x][y]; // 该点表示 障碍物栅格点并且周围一圈存在自由区域

    // 不考虑周围一圈都是障碍物的情况
    if (cell.queueing == fwProcessed)
      continue;

    if (cell.needsRaise) 
    {
      // RAISE
      for (int dx=-1; dx<=1; dx++) 
      {
        int nx = x+dx;
        if (nx <= 0 || nx >= sizeX - 1)
          continue;
        for (int dy = -1; dy <= 1; dy++)
        {
          if (dx == 0 && dy == 0)
            continue;
          int ny = y + dy;
          if (ny <= 0 || ny >= sizeY - 1)
            continue;

          // 1、取出当前节点的邻居节点，找到邻居节点是存在障碍物点坐标，并且是不需要更新的栅格
          // 2、判断当前邻居节点的障碍物点坐标是否存在
          // 若不存在则需要将该邻居节点加入到开集合mOpen中，并且初始化needsRaise为需要去除障碍物的属性，并且将该点的障碍物坐标置空
          // 若存在则需要将该邻居节点加入到开集合mOpen中

          dataCell nc = mData[nx][ny];
          if (nc.obstX != invalidObstData && !nc.needsRaise)
          {
            if (!isOccupied(nc.obstX, nc.obstY, mData[nc.obstX][nc.obstY])){
              mOpen.push(nc.sqdist, INTPOINT(nx, ny));
              nc.queueing = fwQueued;
              nc.needsRaise = true;
              nc.obstX = invalidObstData;
              nc.obstY = invalidObstData;
              if (updateRealDist)
                nc.dist = INFINITY;
              nc.sqdist = INT_MAX;
              mData[nx][ny] = nc;
            }
            else{
              if(nc.queueing != fwQueued){
                mOpen.push(nc.sqdist, INTPOINT(nx,ny));
                nc.queueing = fwQueued;
                mData[nx][ny] = nc;
              }
            }
          }

        }
      }
      cell.needsRaise = false;
      cell.queueing = bwProcessed;  // 已处理
      mData[x][y] = cell;
    }
    else if (cell.obstX != invalidObstData && isOccupied(cell.obstX, cell.obstY, mData[cell.obstX][cell.obstY]))
    {
      // LOWER
      cell.queueing = fwProcessed;  // 标记为已处理的状态
      cell.voronoi = occupied;      // 标记为障碍物

      for (int dx = -1; dx <= 1; dx++)
      {
        int nx = x + dx;
        if (nx <= 0 || nx >= sizeX - 1)
          continue;
        
        for (int dy = -1; dy <= 1; dy++)
        {
          if (dx == 0 && dy == 0)
            continue;

          int ny = y + dy;
          if (ny <= 0 || ny >= sizeY - 1)
            continue;

          dataCell nCell = mData[nx][ny];  // 从地图中取出cell点
          if(!nCell.needsRaise) {
            int distx = nx - cell.obstX;
            int disty = ny - cell.obstY;
            int newSqDistance = distx * distx + disty * disty;  // 计算当前邻居点和当前障碍物点之间的完全平方距离

            // 1.障碍物点的sqdist为0，只有非障碍物点坐标sqdist为无穷大的值
            // 2.判断当前邻居点距离当前点的障碍物点的完全平方距离 和 邻居点距离自己障碍物实际的完全平方距离的大小

            // overwrite为true时表明当前的邻居节点距离障碍物的距离 小于 它距离障碍物原始的完全平方距离的大小
            bool overwrite = (newSqDistance < nCell.sqdist);
            if (!overwrite && newSqDistance == nCell.sqdist){
              if (nCell.obstX == invalidObstData || isOccupied(nCell.obstX, nCell.obstY, mData[nCell.obstX][nCell.obstY]) == false)
                overwrite = true;
            }

            // 1.当overwrite为true时，说明当前点的邻居点更靠近自己的障碍物点，需要对该点的属性进行更形并加入到mOpen中
            if (overwrite) {
                                                             // 2.障碍物点的完全平方距离newSqDistance为0，自由区域的完全平方距离大于0
              mOpen.push(newSqDistance, INTPOINT(nx, ny));   // 3.将当前邻居点加入到开集合中，以及它的完全平方距离
              nCell.queueing = fwQueued;                     // 4.将该点标记为已存在于开集中

              if (updateRealDist) {
                nCell.dist = sqrt((double) newSqDistance);   // 5.更新该点到障碍物的距离值
              }

              nCell.sqdist = newSqDistance;                  // 6.更新该邻居点的到障碍物点的完全平方距离
              nCell.obstX = cell.obstX;                      // 7.更新该邻居点的障碍物根坐标
              nCell.obstY = cell.obstY;
            }
            else {
              checkVoro(x, y, nx, ny, cell, nCell);          // 对voronoi顶点进行筛选(邻居点到自己障碍物点的距离小于它到其它障碍物点距离时)
            }

            mData[nx][ny] = nCell;
          }
        }
      }
    }

    mData[x][y] = cell;
  }

  return;
}

float DynamicVoronoi::getDistance( int x, int y ) {
  if ((x > 0) && (x < sizeX) && (y > 0) && (y < sizeY))
    return mData[x][y].dist;
  else
    return -INFINITY;
}

bool DynamicVoronoi::isVoronoi( int x, int y ) {
  dataCell c = mData[x][y];
  return (c.voronoi == free || c.voronoi == voronoiKeep);
}

bool DynamicVoronoi::isVoronoiAlternative(int x, int y) {
  int v = alternativeDiagram[x][y];
  return (v == free || v == voronoiKeep);
}

// 函数功能:
// 1.将addList中的点对应的mData中的cell进行状态更新
// 2.将removeList中的点对应的mData中的cell进行状态更新
void DynamicVoronoi::commitAndColorize(bool updateRealDist) 
{
  // ADD NEW OBSTACLES
  for (unsigned int i = 0; i < addList.size(); i++)
  {
    INTPOINT p = addList[i];
    int x = p.x;
    int y = p.y;
    dataCell c_add = mData[x][y];

    if(c_add.queueing != fwQueued)
    {
      if (updateRealDist)
        c_add.dist = 0;                 // 更新该点距离障碍物的欧式距离为0
      c_add.sqdist = 0;                 // 更新该点距离障碍物的完全平方距离为0
      c_add.obstX = x;                  // 更新该点的障碍物坐标点
      c_add.obstY = y;
      c_add.queueing = fwQueued;        // 标记为已存在开集中
      c_add.voronoi = occupied;         // 标记为占有状态

      mData[x][y] = c_add;
      mOpen.push(0, INTPOINT(x, y));    // 存入自身是障碍物并且周围一圈存在不是障碍物区域的栅格点，并且sqdist的值为0
    }
  }

  std::cout << "removeList_size_: " << removeList.size() << " addList_size: " << addList.size() << " all: " << sizeX * sizeY << std::endl;

  // REMOVE OLD OBSTACLES
  for (unsigned int i = 0; i < removeList.size(); i++){
    INTPOINT p = removeList[i];
    int x = p.x;
    int y = p.y;
    dataCell c_rm = mData[x][y];

    // 障碍物被移除并重新插入, 当前这个点是没有障碍物坐标的(若存在则跳过)
    if (isOccupied(x, y, c_rm) == true)
      continue;
    
    mOpen.push(0, INTPOINT(x, y));
    if (updateRealDist)
      c_rm.dist = INFINITY;

    c_rm.sqdist = INT_MAX;
    c_rm.needsRaise = true;
    mData[x][y] = c_rm;
  }
  
  removeList.clear();
  addList.clear();
  return;
}

// 函数功能: 更新Voronoi图中的单元格数据
// 输入: 当前坐标和当前坐标对应的cell，以及邻居点坐标和邻居点对应的cell
void DynamicVoronoi::checkVoro(int x, int y, int nx, int ny, dataCell& cell_, dataCell& ncell_) 
{
  // 1.剔除障碍物点，因为障碍物点的sqdist为0
  // 2.剔除非障碍物点的自由区域，因为大部分的自由区域的障碍物坐标点为无穷大(除非该自由区域存在障碍物根坐标)
  if ((cell_.sqdist > 1 || ncell_.sqdist > 1) && ncell_.obstX != invalidObstData)
  {
    // 3.要求当前点的障碍物坐标和它邻居点的障碍物坐标不相同
    if (abs(cell_.obstX - ncell_.obstX) > 1 || abs(cell_.obstY - ncell_.obstY) > 1)
    {
      // 计算从x，y到nx，ny障碍物的sqdist
      int dxy_x = x - ncell_.obstX;
      int dxy_y = y - ncell_.obstY;
      int sqdxy = dxy_x * dxy_x + dxy_y * dxy_y;     // 当前点到邻居节点的障碍物点的完全平方距离
      int stability_xy = sqdxy - cell_.sqdist;       // 当前点到邻居节点的障碍物点的完全平方距离 与 当前点到自身障碍物点的完全平方距离 的差值

      // 1、过滤规则: 当前点距离自己障碍物点的距离 一定小于 当前点距离其它障碍物点的距离
      if (sqdxy - cell_.sqdist < 0)
        return;

      // 计算从nx，ny到x，y障碍物的sqdist
      int dnxy_x = nx - cell_.obstX;
      int dnxy_y = ny - cell_.obstY;
      int sqdnxy = dnxy_x * dnxy_x + dnxy_y * dnxy_y; // 邻居节点到当前点的障碍物点的完全平方距离
      int stability_nxy = sqdnxy - ncell_.sqdist;     // 邻居节点到当前点的障碍物点的完全平方距离 与 邻居节点到自身障碍物点的完全平方距离 的差值

      // 2、过滤规则: 邻居点距离自己障碍物点的距离 一定小于 邻居点距离其它障碍物点的距离
      if (sqdnxy - ncell_.sqdist < 0)
        return;

      static int num_ = 0;
      if (num_ < 0){
        std::cout << "xy: [" << x << " " << y << "], nxy: [" << nx << " " << ny << "], sxy_: " << stability_xy << ", snxy_: " << stability_nxy << " ,[" << sqdxy << " " << sqdnxy << "]";
        std::cout << ", sqdist: [" << cell_.sqdist << " " << ncell_.sqdist << "]\n";
        num_++;
      }

      // 邻居节点和当前节点进行比较，判断那个点为voronoi的顶点，
      // stability_xy的值越大，则说明当前cell的位置越靠近障碍物坐标点，越小则说明当前cell的位置越靠近cell障碍物坐标和邻居障碍物坐标的中点处 (stability_nxy也是如此分析)

      // 1、若stability_xy的值 小于等于stability_nxy 则说明当前cell的位置更靠近两个障碍物的中点处(所以需要将cell位置的坐标设为voronoi点)，但需要排除距离障碍物坐标平方小于等于2的坐标点
      if (stability_xy <= stability_nxy && cell_.sqdist > 2){
        if (cell_.voronoi != free){
          cell_.voronoi = free;
          reviveVoroNeighbors(x, y);
          pruneQueue.push(INTPOINT(x, y));
        }
      }

      // 2、若stability_nxy的值 小于等于stability_xy 则说明当前ncell的位置更靠近两个障碍物的中点处(所以需要将ncell位置的坐标设为voronoi点)，但需要排除距离障碍物坐标平方小于等于2的坐标点
      if (stability_nxy <= stability_xy && ncell_.sqdist > 2){
        if (ncell_.voronoi != free){
          ncell_.voronoi = free;
          reviveVoroNeighbors(nx, ny);
          pruneQueue.push(INTPOINT(nx, ny));
        }
      }
    }
  }

  return;
}


void DynamicVoronoi::reviveVoroNeighbors(int &x, int &y) 
{
  for (int dx = -1; dx <= 1; dx++)
  {
    int nx = x + dx;
    if (nx <= 0 || nx >= sizeX - 1)
      continue;
    for (int dy = -1; dy <= 1; dy++)
    {
      if (dx == 0 && dy == 0)
        continue;
      int ny = y + dy;
      if (ny <= 0 || ny >= sizeY - 1)
        continue;

      dataCell nc = mData[nx][ny];
      if (nc.sqdist != INT_MAX && !nc.needsRaise && (nc.voronoi == voronoiKeep || nc.voronoi == voronoiPrune)) {
        
        static int ssdata = 0;
        if(ssdata < 10){
          std::cout << "#reviveVoroNeighbors_#" << std::endl;
          ssdata++;
        }

        nc.voronoi = free;
        mData[nx][ny] = nc;
        pruneQueue.push(INTPOINT(nx, ny));
      }
    }
  }
  return;
}

bool DynamicVoronoi::isOccupied(int x, int y) {
  dataCell c = mData[x][y];
  return (c.obstX == x && c.obstY == y);
}

// 判断当前的点坐标是否是障碍物点的坐标
bool DynamicVoronoi::isOccupied(int &x, int &y, dataCell &c) {
  return (c.obstX == x && c.obstY == y);
}

std::vector<std::pair<int, int>> DynamicVoronoi::visualize(const char *filename)
{
  std::vector<std::pair<int, int>> vec_dynamic;

  FILE* F = fopen(filename, "w");
  if (!F) {
    std::cerr << "could not open_ 'result.pgm' for writing!\n";
    return vec_dynamic;
  }
  fprintf(F, "P6\n#\n");
  fprintf(F, "%d %d\n255\n", sizeX, sizeY);

  for (int y = sizeY - 1; y >= 0; y--)
  {
    for (int x = 0; x < sizeX; x++)
    {
      if (alternativeDiagram != NULL && (alternativeDiagram[x][y] == free || alternativeDiagram[x][y] == voronoiKeep)){
        std::cout << "##################" << std::endl;
        fputc(255, F);
        fputc(0, F);
        fputc(0, F);
      }
      else if (isVoronoi(x, y)){
        // 此处为voron点
        fputc(0, F);
        fputc(0, F);
        fputc(255, F);
        vec_dynamic.emplace_back(std::make_pair(x, sizeY - y));
      }
      else if (mData[x][y].sqdist == 0){
        // 此处为障碍物区域
        fputc(0, F);
        fputc(0, F);
        fputc(0, F);
      }
      else{
        // 此处为非障碍物区域
        float f = 80 + (sqrt(mData[x][y].sqdist) * 10);
        if (f > 255)
          f = 255;
        if (f < 0)
          f = 0;
        unsigned char color = (unsigned char)f;
        fputc(color, F);
        fputc(color, F);
        fputc(color, F);
      }
    }
  }
  fclose(F);
  return vec_dynamic;
}

void DynamicVoronoi::prune() 
{
  // filler
  while(!pruneQueue.empty()) {
    INTPOINT p = pruneQueue.front();
    pruneQueue.pop();

    int x = p.x;
    int y = p.y;

    // 当前的栅格点不是障碍物并且它不在sortedPruneQueue中
    if (mData[x][y].voronoi == occupied)
      continue;
    if (mData[x][y].voronoi == freeQueued)
      continue;

    mData[x][y].voronoi = freeQueued;
    sortedPruneQueue.push(mData[x][y].sqdist, p);

    /* tl t tr
       l  c r
       bl b br */

    dataCell tr, tl, br, bl;
    tr = mData[x + 1][y + 1];
    tl = mData[x - 1][y + 1];
    br = mData[x + 1][y - 1];
    bl = mData[x - 1][y - 1];

    dataCell r,b,t,l;
    r = mData[x + 1][y];
    l = mData[x - 1][y];
    t = mData[x][y + 1];
    b = mData[x][y - 1];

    if (x + 2 < sizeX && r.voronoi == occupied){
      // fill to the right
      if (tr.voronoi != occupied && br.voronoi != occupied && mData[x + 2][y].voronoi != occupied){
        r.voronoi = freeQueued;
        sortedPruneQueue.push(r.sqdist, INTPOINT(x + 1, y));
        mData[x + 1][y] = r;
      }
    }

    if (x - 2 >= 0 && l.voronoi == occupied){
      // fill to the left
      if (tl.voronoi != occupied && bl.voronoi != occupied && mData[x - 2][y].voronoi != occupied){
        l.voronoi = freeQueued;
        sortedPruneQueue.push(l.sqdist, INTPOINT(x - 1, y));
        mData[x - 1][y] = l;
      }
    }

    if (y + 2 < sizeY && t.voronoi == occupied){
      // fill to the top
      if (tr.voronoi != occupied && tl.voronoi != occupied && mData[x][y + 2].voronoi != occupied){
        t.voronoi = freeQueued;
        sortedPruneQueue.push(t.sqdist, INTPOINT(x, y + 1));
        mData[x][y + 1] = t;
      }
    }

    if (y - 2 >= 0 && b.voronoi == occupied){
      // fill to the bottom
      if (br.voronoi != occupied && bl.voronoi != occupied && mData[x][y - 2].voronoi != occupied){
        b.voronoi = freeQueued;
        sortedPruneQueue.push(b.sqdist, INTPOINT(x, y - 1));
        mData[x][y - 1] = b;
      }
    }
  }


  while(!sortedPruneQueue.empty()) {
    INTPOINT p = sortedPruneQueue.pop();

    dataCell c = mData[p.x][p.y];
    int v = c.voronoi;
    if (v != freeQueued && v != voronoiRetry){
      continue;
    }

    // 根据返回值更新voronoi的状态
    markerMatchResult r = markerMatch(p.x, p.y);
    
    // 更新栅格状态 和 mData
    if (r == pruned)
      c.voronoi = voronoiPrune;
    else if (r == keep)
      c.voronoi = voronoiKeep;  // 新增加的voronoi顶点
    else{ 
      c.voronoi = voronoiRetry;
      pruneQueue.push(p);
    }
    mData[p.x][p.y] = c;

    if (sortedPruneQueue.empty()) 
    {
      while (!pruneQueue.empty()) {
        INTPOINT p = pruneQueue.front();
        pruneQueue.pop();
        sortedPruneQueue.push(mData[p.x][p.y].sqdist, p);
      }
    }
  }

}

// 修改原本的关系图
void DynamicVoronoi::updateAlternativePrunedDiagram() 
{
  if (alternativeDiagram == NULL){
    alternativeDiagram = new int *[sizeX];
    for (int x = 0; x < sizeX; x++){
      alternativeDiagram[x] = new int[sizeY];
    }
  }

  std::queue<INTPOINT> end_cells;               // 储存自由栅格区域的点
  BucketPrioQueue<INTPOINT> sortedPruneQueue_;  // 储存自由区域的点和它距离障碍物的完全平方距离
  for (int x = 1; x < sizeX - 1; x++){
    for (int y = 1; y < sizeY - 1; y++){
      dataCell &c = mData[x][y];
      alternativeDiagram[x][y] = c.voronoi;
      if (c.voronoi <= free)
      {
        sortedPruneQueue_.push(c.sqdist, INTPOINT(x, y));   // 将自由区域加入到精简集合中
        end_cells.push(INTPOINT(x, y));
      }
    }
  }

  // 1.判断某个栅格的周围一圈是否存在3个以上的free和voronoiKeep的点
  for (int x = 1; x < sizeX - 1; x++){
    for (int y = 1; y < sizeY - 1; y++){
      if (getNumVoronoiNeighborsAlternative(x, y) >= 3){
        alternativeDiagram[x][y] = voronoiKeep;
        sortedPruneQueue_.push(mData[x][y].sqdist, INTPOINT(x, y));
        end_cells.push(INTPOINT(x, y));
      }
    }
  }
  int tem = end_cells.size();
  std::cout << "end_cells_size: " << end_cells.size() << std::endl;

  // 2.多增加一些自由栅格区域，以进行筛选，找出符合条件的voronoiPrune
  for (int x = 1; x < sizeX - 1; x++){
    for (int y = 1; y < sizeY - 1; y++){
      if (getNumVoronoiNeighborsAlternative(x, y) >= 3){
        alternativeDiagram[x][y] = voronoiKeep;
        sortedPruneQueue_.push(mData[x][y].sqdist, INTPOINT(x, y));
        end_cells.push(INTPOINT(x, y));
      }
    }
  }
  std::cout << "end_cells_size: " << end_cells.size() << ", than: " << end_cells.size() - tem << std::endl;

  // 3.找到需要修剪的坐标点并标记为 voronoiPrune_
  while (!sortedPruneQueue_.empty())
  {
    INTPOINT p = sortedPruneQueue_.pop();
    if (markerMatchAlternative(p.x, p.y)){
      alternativeDiagram[p.x][p.y] = voronoiPrune;
    }
    else{
      alternativeDiagram[p.x][p.y] = voronoiKeep;
    }
  }

  int flag1 = 0;
  int flag2 = 0;
  // delete worms
  while (!end_cells.empty()) {
    INTPOINT p = end_cells.front();
    end_cells.pop();

    if (isVoronoiAlternative(p.x, p.y) && getNumVoronoiNeighborsAlternative(p.x, p.y) == 1)
    {
      flag1++;
      alternativeDiagram[p.x][p.y] = voronoiPrune;
      for (int dx = -1; dx <= 1; ++dx) {
        for (int dy = -1; dy <= 1; ++dy) {
          if (!(dx || dy) || (dx && dy)) {
            continue;
          }
          int nx = p.x + dx;
          int ny = p.y + dy;
          if (nx < 0 || nx >= sizeX || ny < 0 || ny >= sizeY) {
            continue;
          }
          if (isVoronoiAlternative(nx,ny)) {
            flag2++;
            if (getNumVoronoiNeighborsAlternative(nx, ny) == 1) {
              end_cells.push(INTPOINT(nx, ny));
            }
          }
        }
      }
    }
  }
  std::cout << "flag1: " << flag1 << ", flag2: " << flag2 << std::endl;

  return;
}

// 函数功能: 如果返回true，则进行修剪
// 1.遍历当前栅格坐标周围一圈的栅格坐标，当该栅格值不是障碍物点并且不是voronoiPrune值时，置true
// 2.去除只连接到一个单元格、垂直、水平的情况
bool DynamicVoronoi::markerMatchAlternative(int x, int y) 
{
  bool mf[8];
  int nx, ny;
  int dx, dy;

  int idd = 0;
  int voroCount = 0;  // 统计该栅格周围一圈满足条件的栅格个数
  for (dy = 1; dy >= -1; dy--) {
    ny = y + dy;
    for (dx = -1; dx <= 1; dx++) {
      if (dx || dy) {
        nx = x + dx;
        int v_data = alternativeDiagram[nx][ny];
        bool b_flag = (v_data <= free && v_data != voronoiPrune);
        
        mf[idd] = b_flag;
        if (v_data <= free && !(dx && dy))
          voroCount++;
        idd++;
      }
    }
  }

  // 5 6 7
  // 3   4
  // 0 1 2
  {
    // 1.去除只连接到一个单元格的情况
    if (voroCount == 1 && (mf[1] || mf[3] || mf[4] || mf[6])) {
      return false;
    }

    // 2.去除垂直的情况
    if ((!mf[0] && mf[1] && mf[3]) || (!mf[2] && mf[1] && mf[4]) || (!mf[5] && mf[3] && mf[6]) || (!mf[7] && mf[6] && mf[4]))
      return false;

    // 3.去除水平的情况
    if ((mf[3] && mf[4] && !mf[1] && !mf[6]) || (mf[1] && mf[6] && !mf[3] && !mf[4]))
      return false;
  }
  return true;
}

// 返回该坐标点周围一圈的栅格，它的值是free和voronoiKeep的个数
int DynamicVoronoi::getNumVoronoiNeighborsAlternative(int x, int y) 
{
  int count = 0;
  for (int dx = -1; dx <= 1; dx++) {
    for (int dy = -1; dy <= 1; dy++) {
      if ((dx == 0 && dy == 0) || (dx != 0 && dy != 0)) {
        continue;
      }

      int nx = x + dx;
      int ny = y + dy;
      if (nx < 0 || nx >= sizeX || ny < 0 || ny >= sizeY) {
        continue;
      }
      if (alternativeDiagram[nx][ny] == free || alternativeDiagram[nx][ny] == voronoiKeep){
        count++;
      }
    }
  }

  return count;
}

// 连接模式的实现
DynamicVoronoi::markerMatchResult DynamicVoronoi::markerMatch(int x, int y) 
{
  bool f[8];
  int nx, ny;
  int dx, dy;

  int i = 0;
  int count = 0;
  int voroCount = 0;      // 记录 8 邻阈满足条件的个数
  int voroCountFour = 0;  // 记录 4 邻阈满足条件的个数

  for (dy = 1; dy >= -1; dy--)
  {
    ny = y + dy;
    for (dx = -1; dx <= 1; dx++){
      if (dx || dy){
        nx = x + dx;

        dataCell nc = mData[nx][ny];
        int v = nc.voronoi;
        bool b = (v <= free && v != voronoiPrune);  // 非障碍物区域且没有进行修剪的点

        f[i] = b;
        if (b){
          voroCount++;
          if (!(dx && dy))
            voroCountFour++;
        }

        if (b && !(dx && dy))
          count++;
        i++;
      }
    }
  }

  /*
    f7 f4 f2
    f6    f1
    f5 f3 f0
  */

  // 8邻阈中符合条件的栅格数量小于3 并且 4邻阈中只存在一个非障碍物区域且没有进行修剪的点
  if (voroCount < 3 && voroCountFour == 1 && (f[1] || f[3] || f[4] || f[6])){
    return keep;
  }

  // 4-connected
  if ((!f[0] && f[1] && f[3]) || (!f[2] && f[1] && f[4]) || (!f[5] && f[3] && f[6]) || (!f[7] && f[6] && f[4]))
    return keep;

  if ((f[3] && f[4] && !f[1] && !f[6]) || (f[1] && f[6] && !f[3] && !f[4]))
    return keep;

  // 将voro单元保留在块内，稍后重试
  if (voroCount >= 5 && voroCountFour >= 3 && mData[x][y].voronoi != voronoiRetry){
    return retry;
  }

  return pruned;
}











